package rigidbody;

import static util.Constants.FP_TOLERANCE;
import geometry.Drawable;
import geometry.Triangle;
import geometry.TriangleMesh;
import geometry.TriangleMesh.TriangleIndex;

import java.util.ArrayList;
import java.util.HashSet;
import java.util.List;

import javax.media.opengl.GL2;
import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Point3d;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3d;
import javax.vecmath.Vector4d;

import simulation.ImpulseConstraintSolver;
import util.Util;
import collision.ASDFOctree;
import collision.BoundingVolume;
import collision.Collision;
import collision.OBBTree;

/**
 * The RigidBody class holds the state information for a physical object within
 * the world which needs to be subjected to physics.
 * 
 * @author Jeran Fox
 * @author Matthew Cong
 * @author Teddy Ni
 * @author Yunfeng Bai
 */
public class RigidBody extends SimulationObject
{
    protected Drawable m_geometry;

    /** Next RigidBody ID. */
    protected static int m_nextRigidBodyId = 1;

    /**
     * Linear momentum. We can find velocity from this. It has the advantage of
     * never changing without external force.
     */
    protected Vector3d m_linearMomentum;

    /**
     * Angular momentum. We can find angular velocity from this. It has the
     * advantage of never changing without external torque.
     */
    protected Vector3d m_angularMomentum;

    /** Mass. A rigid body is assumed to have constant mass. */
    protected double m_mass;

    /** Position of object in homogeneous world coordinates. */
    protected Point3d m_position;

    /**
     * Orientation of rigid body, stored as a unit quaternion that rotates from
     * the world frame. We use quaternions, because we anticipate chaining many
     * rotations together.
     */
    protected Quat4d m_orientation;

    /** The differentiation of orientation by time */
    protected Quat4d m_dOrientation;

    /** Flag to control whether the object is at rest. */
    protected boolean m_isResting;

    /** Flag to control whether the object is subject to collisions. */
    protected boolean m_isImmobile;

    protected Vector3d m_accumulatedForce;

    protected Vector3d m_torque;

    //
    // Deducted variables.
    //
    protected Vector3d m_velocity;

    protected Vector3d m_angularVelocity;

    protected Matrix3d m_initialInertia;

    protected Matrix3d m_inertiaAtTimeT;
    
    protected Matrix3d m_inertiaAtTimeTInverse;

    protected Matrix4d m_transformMatrix;

    protected Matrix4d m_transformMatrixTInv;

    protected Matrix4d m_transformMatrixInv;
    
    // back up states
    protected Quat4d m_oldOrientation = new Quat4d();
    protected Point3d m_oldPosition = new Point3d();
    protected Vector3d m_oldAngularMomentum = new Vector3d();
    protected Vector3d m_oldLinearMomentum = new Vector3d();

    /** The bounding volume */
    protected OBBTree m_obbtree;

    /**
     * Creates a new rigid body with the given mass at the given position.
     * 
     * @param double mass of object (in SI units)
     * @param Point3d
     *            location of center of mass (world frame)
     */
    public RigidBody(double mass_, Point3d position_, Drawable geometry_)
    {
        this("Rigid Body " + m_nextRigidBodyId++, mass_, position_, geometry_);
    }

    /**
     * Creates a new rigid body with the given name, the given mass, and the
     * given position.
     * 
     * Nonpositive mass means that the object has infinite mass, and is fixed in
     * the world.
     * 
     * @param String
     *            name that identifies this object
     * @param double mass of object (in SI units)
     * @param Point3d
     *            location of center of mass (world frame)
     */
    public RigidBody(String name_, double mass_, Point3d position_)
    {
        this(name_, mass_, position_, null);
    }

    public RigidBody(String name_, double mass_, Point3d position_,
            Drawable geometry_)
    {
        super(name_);
        setMass(mass_);
        m_isImmobile = false;
        m_geometry = geometry_;
        m_position = new Point3d(position_);
        m_velocity = new Vector3d();
        m_orientation = new Quat4d(0.0, 0.0, 0.0, 1.0);
        m_inertiaAtTimeT = new Matrix3d();
        m_inertiaAtTimeTInverse = new Matrix3d();
        m_angularVelocity = new Vector3d();
        m_accumulatedForce = new Vector3d();
        m_torque = new Vector3d();
        m_linearMomentum = new Vector3d();
        m_angularMomentum = new Vector3d();
        m_initialInertia = new Matrix3d();
        m_transformMatrix = new Matrix4d();
        m_transformMatrixTInv = new Matrix4d();
        m_transformMatrixInv = new Matrix4d();
        m_dOrientation = new Quat4d();
        m_obbtree = new OBBTree(geometry_);
        updateTransformMatrix();
    }
    
    public void initASDF()
    {
    	m_geometry.initASDF(m_obbtree);
    }

    public void clearAccumulatedVars()
    {
        m_torque = new Vector3d();
        m_accumulatedForce = new Vector3d();
    }

    public void accumulateForce(Vector3d l_)
    {
    	if (m_isImmobile) return;
    	
        m_accumulatedForce.add(l_);
    }

    public void accumulateTorque(Point3d pos_, Vector3d force_)
    {
    	if (m_isImmobile) return;
    	
        Vector3d d = new Vector3d();
        d.sub(pos_, m_position);
        d.cross(d, force_);
        m_torque.add(d);
    }

    /**
     * Set position of object (in world frame).
     */
    public void moveTo(double x_, double y_, double z_)
    {
        assert m_position != null;
        m_position.set(x_, y_, z_);
    }
    
    public Vector3d getVelocityAtPointPredicted(Point3d predictedPosInWorld)
    {
    	Vector3d vel = new Vector3d();
    	vel.scale(1.0 / m_mass, m_linearMomentum);
    	
    	Vector3d r = new Vector3d();
    	r.sub(predictedPosInWorld, m_position);
    	Vector3d angularVelocity = calcAngularVelocity();
    	Vector3d angVelocity = new Vector3d();
    	angVelocity.cross(angularVelocity, r);
    	
    	vel.add(angVelocity);
    	
    	return vel;
    }
    
	public Vector3d calcAngularVelocity()
	{
		calculateInertiaInverse();
        m_angularVelocity = (Vector3d) m_angularMomentum.clone();
        m_inertiaAtTimeTInverse.transform(m_angularVelocity);
        return m_angularVelocity;
    }
	
	public Matrix3d calculateInertia()
	{
		Matrix4d rotation = Util.getMatrixFromQuaternion(m_orientation);
        Matrix4d rotationT = (Matrix4d) rotation.clone();
        rotationT.transpose();
        m_inertiaAtTimeT.mul(Util.toMatrix3d(rotation), m_initialInertia);
        m_inertiaAtTimeT.mul(Util.toMatrix3d(rotationT));
        return m_inertiaAtTimeT;
	}

	public Matrix3d calculateInertiaInverse()
	{
		if (m_isImmobile)
		{
			return new Matrix3d();
		}
		
		Matrix4d rotation = Util.getMatrixFromQuaternion(m_orientation);
        Matrix4d rotationT = (Matrix4d) rotation.clone();
        rotationT.transpose();
        m_inertiaAtTimeTInverse.mul(Util.toMatrix3d(rotation), m_initialInertia);
        m_inertiaAtTimeTInverse.mul(Util.toMatrix3d(rotationT));
        m_inertiaAtTimeTInverse.invert();
        return m_inertiaAtTimeTInverse;
	}

    protected void updateTransformMatrix()
    {
        m_transformMatrix.setIdentity();
        m_transformMatrix.setTranslation(new Vector3d(m_position.x, m_position.y,
                m_position.z));
        Matrix4d r = Util.getMatrixFromQuaternion(m_orientation);
        m_transformMatrix.mul(r);
        m_transformMatrixTInv.transpose(m_transformMatrix);
        try
        {
        	m_transformMatrixTInv.invert();
        }
        catch (Exception e)
        {
        	int k = 0;
        }
        m_transformMatrixInv.invert(m_transformMatrix);
    }
    
    public Matrix4d getOldTransformMatrix()
    {
    	Matrix4d m = new Matrix4d();
    	m.setIdentity();
        m.setTranslation(new Vector3d(m_oldPosition.x, m_oldPosition.y,
                m_oldPosition.z));
        Matrix4d r = Util.getMatrixFromQuaternion(m_oldOrientation);
        m.mul(r);
        return m;
    }

    public void updateOBBTree()
    {
    	updateTransformMatrix();
    	m_obbtree.updateTransform(m_transformMatrix, m_transformMatrixInv);
    }

    public void draw(GL2 gl, boolean drawBoundingBox)
    {
        gl.glPushMatrix();
        updateTransformMatrix();
        gl.glMultMatrixd(Util.matrixToArray(m_transformMatrix), 0);
        
        if (drawBoundingBox)
        {
            m_obbtree.draw(gl);
        }
        else
        {
        	m_geometry.draw(gl);
        }
        
        gl.glPopMatrix();
    }
    
    public void backupPosition()
    {
    	m_oldPosition.set(m_position);
    	m_oldOrientation.set(m_orientation);
    }
    
    public void restorePosition()
    {
    	m_position.set(m_oldPosition);
    	m_orientation.set(m_oldOrientation);
    }
    
    public void backupVelocity()
	{
    	m_oldAngularMomentum.set(m_angularMomentum);
    	m_oldLinearMomentum.set(m_linearMomentum);
	}
    
    public void restoreVelocity()
	{
    	m_angularMomentum.set(m_oldAngularMomentum);
    	m_linearMomentum.set(m_oldLinearMomentum);
	}

    /**
     * Get the untransformed triangle mesh.
     */
    public TriangleMesh getTriangleMesh()
    {
        return m_geometry.getTriangleMesh();
    }

    public boolean isResting()
    {
        return m_isResting;
    }

    public void setResting(boolean isResting_)
    {
        m_isResting = isResting_;
    }

    public boolean isImmobile()
    {
        return m_isImmobile;
    }

    public void setIsImmobile(boolean isImmobile_)
    {
        m_isImmobile = isImmobile_;
    }

    public void setOrientation(Quat4d o_)
    {
        m_orientation = o_;
    }

    public Quat4d getOrientation()
    {
        return m_orientation;
    }

    public Vector3d getVelocity()
    {
    	m_velocity.scale(1.0 / m_mass, m_linearMomentum);
        return m_velocity;
    }

    public Quat4d getDOrientation()
    {
    	calcAngularVelocity();
    	if (m_angularVelocity.x == 0 && m_angularVelocity.y == 0
                && m_angularVelocity.z == 0)
        {
            m_dOrientation = new Quat4d();
        }
        else
        {
            m_dOrientation.set(new double[]
            { m_angularVelocity.x, m_angularVelocity.y, m_angularVelocity.z, 0 });
            m_dOrientation.mul(m_orientation);
            m_dOrientation.scale(0.5);
        }
    	return m_dOrientation;
    }

    public double getMass()
    {
        assert m_mass > FP_TOLERANCE;
        return m_mass;
    }
    
    public double getMassInv()
    {
    	if (m_isImmobile) return 0;
    	else return 1.0 / m_mass;
    }

    /**
     * Set mass of object. Nonpositive mass means infinite mass.
     * 
     * @param double object mass (in SI units)
     */
    public void setMass(double mass_)
    {
        m_mass = (mass_ > FP_TOLERANCE) ? mass_ : Double.POSITIVE_INFINITY;
    }

    /**
     * Get position of object (in world frame and homogeneous coordinates).
     */
    public final Point3d getPosition()
    {
        assert m_position != null;
        return m_position;
    }

    public void setPosition(Point3d p_)
    {
        m_position = p_;
    }

    public void setGeometry(Drawable geometry_)
    {
        m_geometry = geometry_;
    }

    public Drawable getGeometry()
    {
        return m_geometry;
    }
    
    public void setInertia(Matrix3d inertia_)
    {
        m_initialInertia = inertia_;
    }
    
    public Vector3d getLinearMomentum()
    {
        return m_linearMomentum;
    }
    
    public Vector3d getAngularVelocity()
    {
    	return m_angularVelocity;
    }
    
    public Vector3d getAngularMomentum()
    {
        return m_angularMomentum;
    }
    
    public void setAngularMomentum(Vector3d angularMomentum)
    {
    	this.m_angularMomentum = angularMomentum;
    }
    
    public void setLinearMomentum(Vector3d linearMomentum)
    {
    	this.m_linearMomentum = linearMomentum;
    }

    public Vector3d getAccumulatedTorque()
    {
        return m_torque;
    }

    public Vector3d getAccumulatedForce()
    {
        return m_accumulatedForce;
    }

    public BoundingVolume getBoundingVolume()
    {
    	updateOBBTree();
        return m_obbtree;
    }
    
    public OBBTree getOBBTree()
    {
    	updateOBBTree();
    	return m_obbtree;
    }

    public Matrix4d getTransformMatrix()
    {
    	updateTransformMatrix();
        return m_transformMatrix;
    }

    public Matrix4d getTransformMatrixInverse()
    {
    	updateTransformMatrix();
        return m_transformMatrixInv;
    }

    public ASDFOctree getASDFTree()
    {
        return m_geometry.getASDFTree();
    }

	public Matrix3d getK(Vector3d normal, Point3d predictedPositionInWorld)
	{
		if (m_isImmobile) return new Matrix3d();
		
		Vector3d r = new Vector3d();
		r.sub(predictedPositionInWorld, m_position);
		
		Matrix3d rcross = getCrossProductMatrix(r);
		Matrix3d rcrossT = new Matrix3d();
		rcrossT.transpose(rcross);
		
		Matrix3d inertiaAtTimeTInverse = calculateInertiaInverse();
		
		rcrossT.mul(inertiaAtTimeTInverse);
		rcrossT.mul(rcross);

		// The mass matrix part.
		rcrossT.m00 += 1 / m_mass;
		rcrossT.m11 += 1 / m_mass;
		rcrossT.m22 += 1 / m_mass;
		
		return rcrossT;
	}
	
	private Matrix3d getCrossProductMatrix(Vector3d v)
	{
		Matrix3d m = new Matrix3d(0, -v.z, v.y,
								v.z, 0, -v.x,
								-v.y, v.x, 0);
		return m;
	}
	
	/**
	 * This function add impulse to the rigidbody and change
	 * linear momentum and angular momentum accordingly.
	 */
	public void applyImpulseToCurrent(Vector3d p, Point3d positionInWorld)
	{
		if (m_isImmobile) return;
		
		m_oldLinearMomentum.add(p);
		
		Vector3d r = new Vector3d();
		r.sub(positionInWorld, m_position);
		Vector3d d = new Vector3d();
		d.cross(r, p);
		m_oldAngularMomentum.add(d);
	}
	
	public void applyImpulse(Vector3d p, Point3d positionInWorld)
	{
		if (m_isImmobile) return;
		
		m_linearMomentum.add(p);
		
		Vector3d r = new Vector3d();
		r.sub(positionInWorld, m_position);
		Vector3d d = new Vector3d();
		d.cross(r, p);
		m_angularMomentum.add(d);
	}
	
	/**
	 * Check if any point in the other rigid body has hit me.
	 */
	public List<Collision> detectCollisionWith(RigidBody b)
	{
		List<Collision> coll = new ArrayList<Collision>();
		
		TriangleMesh mesh1 = b.getTriangleMesh();
		for (Point3d p : mesh1._vertices)
		{
			// Transform every point from b (in model space) into this object's model space,
			// then calculate the signed distance with ASDF tree.
		    Point3d ptrans = new Point3d(p);
		    Vector3d normal = new Vector3d();
		    b.getTransformMatrix().transform(ptrans);
		    getTransformMatrixInverse().transform(ptrans);
			double distance = getASDFTree().getDistance(ptrans, normal);
//			System.out.println(distance);
			if (distance >= -util.Constants.FP_TOLERANCE) continue;
			
			getTransformMatrix().transform(ptrans);
			
			// Normal from nearest triangle.
//			Vector3d normalFromTriangle = new Vector3d();
//			Triangle tmpTriangle = new Triangle();
//			Point3d targetPoint = new Point3d();
//			distance = getTriangleMesh().distanceToPoint(ptrans, tmpTriangle, targetPoint);
//	        getTransformMatrix().transform(targetPoint);
//	        normalFromTriangle.sub(ptrans, targetPoint);
//	        normalFromTriangle.normalize();
			
	        // Normal from the ASDF.
	        Vector4d tmpNormal = new Vector4d(normal.x, normal.y, normal.z, 0);
	        getTransformMatrix().transform(tmpNormal);
	        normal.x = tmpNormal.x;
	        normal.y = tmpNormal.y;
	        normal.z = tmpNormal.z;
			normal.negate();
			normal.normalize();
			
			coll.add(new Collision(distance, b, this, ptrans, normal));
		}
		
		return coll;
	}
	
	private static Matrix4d getTransformFromB2ToB1(RigidBody b1, RigidBody b2)
	{
		Matrix4d m = new Matrix4d();
		m.mul(b1.getTransformMatrixInverse(), b2.getTransformMatrix());
		return m;
	}

	/**
	 * Get all the collision pairs between rigidbody b1 and b2.
	 * @param nonSeparating If true, then only return all the non-separating pairs.
	 * @return
	 */
	public static List<Collision> getCollisionsBetween(RigidBody b1, RigidBody b2,
			boolean nonSeparating)
	{
//		if (b1 instanceof BoxRigidBody && b2 instanceof BoxRigidBody)
//		{
//			List<Collision> l1 = b1.detectCollisionWith(b2);
//			l1.addAll(b2.detectCollisionWith(b1));
//			return l1;
//		}
		
		long time1, time2;
		
		List<Collision> collisions = new ArrayList<Collision>();

		// Get all intersecting triangles.
		HashSet<Triangle> triangles1 = new HashSet<Triangle>();
		HashSet<Triangle> triangles2 = new HashSet<Triangle>();
		
		time1 = System.nanoTime();
		OBBTree tree1 = b1.getOBBTree();
		OBBTree tree2 = b2.getOBBTree();
		
		time2 = System.nanoTime();
		ImpulseConstraintSolver.initOBBTreeTime += time2 - time1;
		
		time1 = System.nanoTime();
		
		// Get possibly colliding triangles.
		tree1.collide(tree2, triangles1, triangles2);
		
		time2 = System.nanoTime();
		ImpulseConstraintSolver.OBBCollisionTime += time2 - time1;
		
		// Get penetration points from b1 into b2.
		getUniqueCollisions(b1, b2, collisions, triangles1, nonSeparating);
		
		// Get penetration points from b2 into b1.
		getUniqueCollisions(b2, b1, collisions, triangles2, nonSeparating);
		
		time1 = System.nanoTime();
		ImpulseConstraintSolver.getPenetrationPointsTime += time1 - time2;
		
		return collisions;
	}

	/**
	 * Check the signed distance of every vertex in possibly colliding triangles,
	 * identify those as penetration points (Collisions) if the distance is negative,
	 * as well where the relative velocity is not separating the two objects.
	 */
	private static void getUniqueCollisions(RigidBody b1,
			RigidBody b2, List<Collision> collisions,
			HashSet<Triangle> triangles, boolean nonSeparating)
	{
		HashSet<Integer> vertexIndexes = new HashSet<Integer>();
		Matrix4d m1to2 = getTransformFromB2ToB1(b2, b1);
		
		for (Triangle t : triangles)
		{
			TriangleIndex triangleIndex = t.getTriangleIndex();
			for (int m = 0; m < 3; m++)
			{
				int vertexIndex = triangleIndex.GetIndex(m);
				if (vertexIndexes.contains(vertexIndex)) continue;
				vertexIndexes.add(vertexIndex);
				
				Point3d p = new Point3d(t.getP(m));
				m1to2.transform(p);
				Vector3d normal = new Vector3d();
				double dist = b2.getASDFTree().getDistance(p, normal);
				if (dist < -util.Constants.FP_TOLERANCE)
				{
					b2.getTransformMatrix().transform(p);
					b2.getTransformMatrix().transform(normal);
					normal.negate();
					normal.normalize();
					
					if (nonSeparating)
					{
						// Points where relative speed is separating are not collision points.
						Vector3d urel = getRelativeVelocityAtPoint(p, b1, b2);
						if (urel.dot(normal) <= 0) continue;
					}
					
					collisions.add(new Collision(-dist, b1, b2, p, normal));
				}
			}
		}
	}

	/**
	 * Get the relative speed from b2 to b1.
	 */
	public static Vector3d getRelativeVelocityAtPoint(Point3d positionInWorld, RigidBody b1, RigidBody b2)
	{
		Vector3d urel = new Vector3d();
		urel.sub(b1.getVelocityAtPointPredicted(positionInWorld), b2.getVelocityAtPointPredicted(positionInWorld));
		return urel;
	}
	
	public void integratePosition(double timestep)
	{
		Vector3d dp = new Vector3d();
		dp.scale(timestep, getVelocity());
		getPosition().add(dp);
		
		Quat4d dq = new Quat4d();
		dq.scale(timestep, getDOrientation());
		getOrientation().add(dq);
		getOrientation().normalize();
	}
	
	public void integrateVelocity(double timestep)
	{
		// For linear momentum.
		Vector3d dl = new Vector3d();
		dl.scale(timestep, getAccumulatedForce());
		getLinearMomentum().add(dl);
		
		// For angular momentum.
		Vector3d da = new Vector3d();
		da.scale(timestep, getAccumulatedTorque());
		getAngularMomentum().add(da);
	}
	
	public void addMomentumFromVelocityDelta(Vector3d lin, Vector3d ang)
	{
		if (m_isImmobile) return;
		
		m_linearMomentum.scaleAdd(m_mass, lin, m_linearMomentum);
		Vector3d deltaAng = new Vector3d(ang);
		Matrix3d inertia = calculateInertia();
		inertia.transform(deltaAng);
		m_angularMomentum.add(deltaAng);
		
		if (Double.isNaN(m_angularMomentum.x)
			|| Double.isNaN(m_angularMomentum.y)
			|| Double.isNaN(m_angularMomentum.z))
		{
			int k = 0;
		}
	}
	
	@Override
	public String toString()
	{
		return String.format(
				"Rigidbody id %d name %s pos %s linearMomentum %s angularMomentum %s",
				this.getId(), this.getName(),
				this.getPosition(), this.getLinearMomentum(), this.getAngularMomentum());
	}
}
